#include <mylib/device/tof200f.h>
#include <mylib/utility/debug.h>
#include <mylib/utility/math.h>
#include <stdint.h>
#include <stdio.h>


#define TOFXXF_CMD_READ 0x03
#define TOFXXF_CMD_WRITE 0x06
#define TOFXXF_REG_SPECIAL 0x0001

UART_HandleTypeDef* g_tofxxf_huart = NULL;

// 发送命令
static inline void tofxxf_send_cmd(uint8_t* cmd, uint8_t len)
{
    HAL_UART_Transmit(g_tofxxf_huart, cmd, len, 1000);
}

void modbus_send_data(uint8_t* cmd, uint8_t len)
{
    HAL_UART_Transmit(g_tofxxf_huart, cmd, len, 1000);
}

void modbus_recv_data(uint8_t* buf, uint8_t len)
{
    HAL_UART_Receive(g_tofxxf_huart, buf, len, 1000);
}

// 往特定moddus从机的寄存器内写入某个值
bool modbus_write_reg(uint8_t address, uint16_t reg, uint16_t value, bool use_check)
{
    uint8_t  cmd[8] = {address,
                       TOFXXF_CMD_WRITE,
                       BITS_GET_HIGH_BIT_IN_WORD(reg),
                       BITS_GET_LOW_BIT_IN_WORD(reg),
                       BITS_GET_HIGH_BIT_IN_WORD(value),
                       BITS_GET_LOW_BIT_IN_WORD(value)};
    uint16_t crc16  = crc16_modbus_calculate(cmd, 6);
    cmd[6]          = BITS_GET_LOW_BIT_IN_WORD(crc16);
    cmd[7]          = BITS_GET_HIGH_BIT_IN_WORD(crc16);

    modbus_send_data(cmd, 8);

    if (use_check) {
        // 对于回发消息进行校验
        uint8_t result[8];
        modbus_recv_data(result, 8);
        for (uint8_t i = 0; i < 8; i++) {
            if (result[i] != cmd[i]) {
                return false;
            }
        }
    }

    return true;
}

// 读取一个寄存器的值
bool modbus_read_reg(uint8_t address, uint16_t reg, uint16_t value) {}


// 初始化tofxxf
void tofxxf_init(UART_HandleTypeDef* huart)
{
    g_tofxxf_huart = huart;
}

// 扫描tofxxf设备，通过变量返回设备地址，如果成功则address就是地址，且返回true。失败返回false，且address不会写入任何值
bool tofxxf_scan_device(uint8_t* address)
{
    // 通过广播帧来扫描设备
    // uint8_t  cmd[8] = {0x01,
    //                    TOFXXF_CMD_WRITE,
    //                    GET_HIGH_BIT_IN_WORD(TOFXXF_REG_SPECIAL),
    //                    GET_LOW_BIT_IN_WORD(TOFXXF_REG_SPECIAL),
    //                    GET_HIGH_BIT_IN_WORD(test_data),
    //                    GET_LOW_BIT_IN_WORD(test_data)};
    // uint16_t crc16  = crc16_modbus_calculate(cmd, 6);
    // cmd[6]          = GET_LOW_BIT_IN_WORD(crc16);
    // cmd[7]          = GET_HIGH_BIT_IN_WORD(crc16);

    // tofxxf_send_cmd(cmd, 8);
    return true;

    // // 测试通信使用的数据
    // const uint16_t test_data = 0x0000;
    // for (uint16_t try_addr = 1; try_addr <= 0xff; try_addr++) {
    //     uint8_t cmd[8] = {
    //         try_addr & 0xff,
    //         TOFXXF_CMD_WRITE,
    //         GET_HIGH_BIT_IN_WORD(TOFXXF_REG_SPECIAL),
    //         GET_LOW_BIT_IN_WORD(TOFXXF_REG_SPECIAL),
    //         GET_HIGH_BIT_IN_WORD(test_data),
    //         GET_LOW_BIT_IN_WORD(test_data)
    //     };
    //     uint16_t crc16 = crc16_modbus_calculate(cmd, 6);
    //     cmd[6] = GET_LOW_BIT_IN_WORD(crc16);
    //     cmd[7] = GET_HIGH_BIT_IN_WORD(crc16);

    //     tofxxf_send_cmd(cmd, 8);
    // }
}

// 恢复默认参数
void tofxxf_reset(void)
{
    uint8_t TOFXXF_RESET_CMD[] = {0x01, 0x06, 0x02, 0x06, 0x00, 0x01, 0x10, 0x00, 0xD5, 0xF9};
    tofxxf_send_cmd(TOFXXF_RESET_CMD, sizeof(TOFXXF_RESET_CMD));
}
// 重启设备
void tofxxf_restart(void) {}
// 测试通讯
void tofxxf_test(void) {}
// 设置量程模式，0为默认，1为高精度，2位长距离，3为高速
void tofxxf_set_range_mode(uint8_t mode) {}

// 读测量距离命令
#define REG_RD_ADDR 0x0010   // 测量结果寄存器

volatile int     flag = 0;
volatile int32_t tofxxf_distance;

// 获取测量值
int32_t tofxxf_get_distance(void)
{
    // uint8_t a_testBuff[20];
    // a_testBuff[0]  = 0;                  // 广播地址
    // a_testBuff[1]  = 3;                  // 读命令码
    // a_testBuff[2]  = REG_RD_ADDR >> 8;   // regH
    // a_testBuff[3]  = REG_RD_ADDR;        // regL
    // a_testBuff[4]  = 0;                  // numH(仅支持读单个寄存器)
    // a_testBuff[5]  = 1;                  // numL
    // uint16_t crc16 = 0;
    // crc16          = crc16_calculate_modbus(a_testBuff, 6);
    // a_testBuff[6]  = crc16;        // crc16l
    // a_testBuff[7]  = crc16 >> 8;   // crc16h

    // tofxxf_send_cmd(a_testBuff, 8);
    //     uint8_t tmp[10];
    //     DEBUG_INFO("tofxxf_get_distance begin");
    //     HAL_UART_Receive(g_tofxxf_huart, tmp, 1, 1000);
    //     uint16_t readData = 0;
    //     readData          = tmp[3];   // 读回数据H
    //     readData <<= 8;
    //     readData |= tmp[4];   // 读回数据H
    //     tofxxf_distance = readData;
    // DEBUG_INFO("tofxxf_get_distance end");
    // return readData;

    return 0;
}

uint8_t g_buffer[32];
int32_t g_buffer_len = 0;
uint8_t g_low_buf, g_high_buf;
int32_t g_state    = 0;
bool    begin_flag = false;

// 中断解析数据
void tofxxf_irq_handler(void)
{
    if (__HAL_UART_GET_FLAG(g_tofxxf_huart, UART_FLAG_RXNE) != RESET) {
        uint8_t tmp;
        HAL_UART_Receive(g_tofxxf_huart, &tmp, 1, 1000);
        // DEBUG_INFO("tmp = %X", tmp);
        if (tmp == 0x01 && g_state == 0) {
            g_state = 1;
        }
        else if (tmp == 0x03 && g_state == 1) {
            g_state = 2;
        }
        else if (tmp == 0x02 && g_state == 2) {
            g_state = 3;
        }
        else if (g_state == 3) {
            g_high_buf = tmp;
            g_state    = 4;
        }
        else if (g_state == 4) {
            g_low_buf         = tmp;
            uint16_t readData = 0;
            readData          = g_high_buf;   // 读回数据H
            readData <<= 8;
            readData |= g_low_buf;   // 读回数据L
            g_state = 0;
            DEBUG_INFO("readData = %d", readData);
        }
        else {
            g_state = 0;
        }
    }
    if (__HAL_UART_GET_FLAG(g_tofxxf_huart, UART_FLAG_IDLE) != RESET) {
        __HAL_UART_CLEAR_IDLEFLAG(g_tofxxf_huart);   // 清除UART总线空闲中断
    }
}
